
package edu.ortn.wildbots;

import edu.ortn.wildbots.commands.AutomaticLauncherControl;
import edu.ortn.wildbots.commands.CommandBase;
import edu.ortn.wildbots.commands.ManualLauncherControl;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;

public class OI {
    
    private Joystick stick_left;
    private Joystick stick_right;
    
    private boolean launcher_manual = true;
    
    public OI () {
        stick_left = new Joystick(RobotMap.LEFT_JOY);
        stick_right = new Joystick(RobotMap.RIGHT_JOY);
        
        
        JoystickButton launcher_toggle = new JoystickButton(stick_left, 2);
        
        launcher_toggle.whenPressed(new Command() {

            protected void initialize() {
            }

            protected void execute() {
                if (launcher_manual)
                    launcher_manual = true;
                else
                    launcher_manual = true;
            }

            protected boolean isFinished() {
                return true;
            }

            protected void end() {
            }

            protected void interrupted() {
            }
            
        });
        
        JoystickButton fire = new JoystickButton(stick_left, 11);
        fire.whenPressed(new Command() {

            protected void initialize() {
            }

            protected void execute() {
                
                if (launcher_manual) {
                    Command newCommand = new ManualLauncherControl();
                    newCommand.start();
                    Scheduler.getInstance().add(newCommand);
                    
                }
                else {
                    Command newCommand = new AutomaticLauncherControl();
                    newCommand.start();
                    Scheduler.getInstance().add(newCommand);
                }
            }

            protected boolean isFinished() {
                return true;
            }

            protected void end() {
            }

            protected void interrupted() {
            }
            
        });
        
        JoystickButton warmMotors = new JoystickButton(stick_left, 1);
        warmMotors.whenPressed(new WarmCommand());
        warmMotors.whenReleased(new CoolCommand());
    }
    
    public Joystick getJoystickLeft() {
        return stick_left;
    }
    
    public Joystick getJoystickRight() {
        return stick_right;
    }
    
    private class WarmCommand extends CommandBase {
            
            public WarmCommand() {
                requires(launcher);
            }

            protected void initialize() {
            }

            protected void execute() {
                double desired =(stick_left.getZ()+1)/2;
                launcher.setSpeed(desired);
            }

            protected boolean isFinished() {
                return true;
            }

            protected void end() {
                
            }

            protected void interrupted() {
            }
            
        }
    
    private class CoolCommand extends CommandBase {
            
            public CoolCommand() {
                requires(launcher);
            }

            protected void initialize() {
            }

            protected void execute() {
                launcher.setSpeed(0.0);
            
        
            }

            protected boolean isFinished() {
                return true;
            }

            protected void end() {
                
            }

            protected void interrupted() {
            }
            
        }
}

